
/* mainRob.C
 *
 * Basic Robot Agent
 * Very simple version for demonstration
 *
 * For more information about the CiberRato Robot Simulator 
 * please see http://microrato.ua.pt/ or contact us.
 */

#include <math.h>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>

#include "RobSock.h"

/* Calculate the power of left and right motors */
void DetermineAction(int beaconToFollow, float *lPow, float *rPow)
{
    static int counter=0;

    bool   beaconReady;
    static struct beaconMeasure beacon;
    static float  left, right, center;
    static int    Ground;
    static bool   Collision;
    
    /*Access to values from Sensors - Only ReadSensors() gets new values */
    if(IsObstacleReady(LEFT))
        left=     GetObstacleSensor(LEFT);
    if(IsObstacleReady(RIGHT))
        right=    GetObstacleSensor(RIGHT);
    if(IsObstacleReady(CENTER))
        center=   GetObstacleSensor(CENTER);

    beaconReady = IsBeaconReady(beaconToFollow);
    if(beaconReady) {
       beacon =  GetBeaconSensor(beaconToFollow);
    }
    else beaconReady=0;

    if(IsGroundReady())
        Ground=    GetGroundSensor();
    if(IsBumperReady())
        Collision= GetBumperSensor();

    if(center>4.5 || right>4.5 || left>4.5 || Collision) { /* Close Obstacle - Rotate */
        if(counter % 400 < 200) {
           *lPow=0.06;
           *rPow=-0.06; }
        else {
           *lPow=-0.06;
           *rPow=0.06; }
    }
    else if(right>1.5) { /* Obstacle Near - Avoid */
        *lPow=0.0;
        *rPow=0.05;
    }
    else if(left>1.5) {
        *lPow=0.05;
        *rPow=0.0;
    }
    else { 
        if(beaconReady && beacon.beaconVisible && beacon.beaconDir>20.0) { /* turn to Beacon */
           *lPow=0.0;
           *rPow=0.1;
        }
        else if(beaconReady && beacon.beaconVisible && beacon.beaconDir<-20.0) {
           *lPow=0.1;
           *rPow=0.0;
        }
        else { /* Full Speed Ahead */
           *lPow=0.1;
           *rPow=0.1;
        }
    }

    counter++;
}

#define RUN        1
#define STOP       2
#define RETURN     3
#define FINISHED   4

int main(int argc, char *argv[])
{
    char host[100]="localhost";
    char rob_name[20]="robsample";
    float lPow,rPow;
    int state=STOP, stoppedState=RUN, rob_id = 1;
    int beaconToFollow=0;

    printf( " Sample Robot\n Copyright (C) 2001-2008 Universidade de Aveiro\n" );

     /* processing arguments */
    while (argc > 2) /* every option has a value, thus argc must be 1, 3, 5, ... */
    {
        if (strcmp(argv[1], "-host") == 0)
        {
           strncpy(host, argv[2], 99);
           host[99]='\0';
        }
        else if (strcmp(argv[1], "-robname") == 0)
        {
           strncpy(rob_name, argv[2], 19);
           rob_name[19]='\0';
        }
        else if (strcmp(argv[1], "-pos") == 0)
        {
            if(sscanf(argv[2], "%d", &rob_id)!=1)
               argc=0; /* error message will be printed */
        }
        else
        {
                break; /* the while */
        }
        argc -= 2;
        argv += 2;
    }

    if (argc != 1)
    {
        fprintf(stderr, "Bad number of parameters\n"
                "SYNOPSIS: mainRob [-host hostname] [-robname robotname] [-pos posnumber]\n");

        return 1;
    }

    /* Connect Robot to simulator */
    if(InitRobot(rob_name, rob_id, host)==-1)
    {
       printf( "%s Failed to connect\n", rob_name); 
       exit(1);
    }
    printf( "%s Connected\n", rob_name );
    state=STOP;
    while(1)
    {
        /* Reading next values from Sensors */
        ReadSensors();

        if(GetFinished()) /* Simulator has received Finish() or Robot Removed */
        {
           printf(  "%s Exiting\n", rob_name );
           exit(0);
        }
        if(state==STOP && GetStartButton()) state=stoppedState;  /* Restart     */
        if(state!=STOP && GetStopButton())  {
            stoppedState=state;
            state=STOP; /* Interrupt */
        }
        if(state!=STOP && GetReturningLed()) {
            state=RETURN;
            stoppedState=RETURN;
        }

        switch (state) { 
             case RUN:    /* Go */
                 if(GetGroundSensor()==beaconToFollow) {         /* Visit target */
                     SetVisitingLed(1);
                     printf("%s visited target %d at %d\n", rob_name, beaconToFollow, GetTime());
                     beaconToFollow++;
                 }
                 else if(beaconToFollow==GetNumberOfBeacons()) { /* Start Returning */
                     SetReturningLed(1);
                     printf("%s Start returning at %d\n", rob_name, GetTime());
                 }
                 else {
                    //turn off visiting Led
                    if(GetVisitingLed()) SetVisitingLed(0); 

                    DetermineAction(beaconToFollow,&lPow,&rPow);
                    DriveMotors(lPow,rPow);
                 }
                 break;
             case RETURN: /* return */
                 //turn off visiting Led
                 if(GetVisitingLed()) SetVisitingLed(0); 

                 DetermineAction(beaconToFollow,&lPow,&rPow); //beaconToFollow is ignored
                 DriveMotors(lPow,rPow);
                 break;
	}

	//Request Sensors for next cycle
	RequestSensors(3, "IRSensor0", "IRSensor1", "IRSensor2");
	if(GetTime() % 2 ==0) {
		RequestGroundSensor();
	}
	else {
	        RequestBeaconSensor(beaconToFollow);
	}

    }
    return 1;
}

